#include "./Mecanum_AGV.hpp"

arm_matrix_instance_f32 Mecanum_AGV_Class::A_matrix; /* 麦轮逆运动学矩阵 */
arm_matrix_instance_f32 Mecanum_AGV_Class::v_agv_matrix;
arm_matrix_instance_f32 Mecanum_AGV_Class::v_motor_matrix;
float Mecanum_AGV_Class::A_matrix_data[12];
float Mecanum_AGV_Class::agv_v_data[3];
float Mecanum_AGV_Class::motor_v_data[4];

extern Motor_Class servo[4];

void Mecanum_AGV_Class::Init(float distance_x, float distance_y, float max_velocity, float max_rpm)
{
    float distance_temp = (distance_x + distance_y) / 2.0f;
    arm_mat_init_f32(&A_matrix, 4, 3, A_matrix_data);
    arm_mat_init_f32(&v_agv_matrix, 3, 1, agv_v_data);
    arm_mat_init_f32(&v_motor_matrix, 4, 1, motor_v_data);

    A_matrix_data[0] = -1.0f;
    A_matrix_data[1] = 1.0f;
    A_matrix_data[2] = distance_temp;
    A_matrix_data[3] = 1.0f;
    A_matrix_data[4] = 1.0f;
    A_matrix_data[5] = -distance_temp;
    A_matrix_data[6] = -1.0f;
    A_matrix_data[7] = 1.0f;
    A_matrix_data[8] = -distance_temp;
    A_matrix_data[9] = 1.0f;
    A_matrix_data[10] = 1.0f;
    A_matrix_data[11] = distance_temp;

    Motor_Class::Servo_Init(max_velocity, max_rpm); /* 初始化电机驱动器 */
}

void Mecanum_AGV_Class::Write_Velocity(const Velocity_Class &velocity_inagv)
{
    agv_v_data[0] = velocity_inagv.velocity_x;
    agv_v_data[1] = velocity_inagv.velocity_y;
    agv_v_data[2] = velocity_inagv.Trans_Rad(velocity_inagv.velocity_angle);

    arm_mat_mult_f32(&A_matrix, &v_agv_matrix, &v_motor_matrix);
    for (int i = 0; i < 4; i++)
    {
        servo[i].Set_Velocity(v_motor_matrix.pData[i]);
    }
}

void Mecanum_AGV_Class::Read_Velocity(float (&velocity)[4])
{
    for (int i = 0; i < 4; i++)
    {
        velocity[i] = servo[i].current_velocity;
    }
}

unsigned short Mecanum_AGV_Class::Read_Motor_Mode(void)
{
    unsigned short value = 0;
    enum Motor_Class::Motor_State_Enum state;
    for (int i = 0; i < 4; i++)
    {
        state = servo[i].Read_State();
        value |= (char)(state) << (i << 2);
    }
    return value;
}

void Mecanum_AGV_Class::Write_Zero_Velocity(void)
{
    for (int i = 0; i < 4; i++)
    {
        servo[i].Set_Velocity(0);
    }
}

void Mecanum_AGV_Class::Clear_Wrong_Code(void)
{
    for (int i = 0; i < 4; i++)
    {
        servo[i].Clear_Wrong();
    }
}
